Constrained Motion Interpolation with Distance Constraints

We present a novel constraint-based motion interpolation algorithm to improve the performance of local planners in sample-based motion planning. Given two free-space configurations of a robot, our algorithm computes a one-dimensional trajectory subject to distance constraints between the closest features of the robot and the obstacles. We derive simple and closed form solutions to compute a path that guarantees no collisions between these closest features. The resulting local planner is fast and can improve the performance of sample-based planners with no changes to the underlying sampling strategy. In practice, we observe speedups on benchmarks for rigid robots with narrow passages.